package com.example.icar;

import androidx.appcompat.app.AlertDialog;
import androidx.appcompat.app.AppCompatActivity;

import android.content.SharedPreferences;
import android.graphics.Bitmap;
import android.os.Bundle;
import android.os.Handler;
import android.os.Looper;
import android.util.Log;
import android.view.MotionEvent;
import android.view.View;
import android.widget.Button;
import android.widget.CheckBox;
import android.widget.ImageView;
import android.widget.RadioButton;
import android.widget.RadioGroup;
import android.widget.TextView;
import android.widget.Toast;
import android.widget.EditText;

import com.example.icar.rosmaster.RosmasterCamera;
import com.example.icar.rosmaster.RosmasterLib;
import com.example.icar.rosmaster.RosmasterHttpClient;

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStream;
import java.io.PrintWriter;
import java.net.Socket;
import java.net.HttpURLConnection;
import java.net.URL;
import java.text.ParseException;
import java.text.SimpleDateFormat;
import java.util.Date;
import java.util.Locale;
import java.util.TimeZone;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import android.content.pm.ActivityInfo;

public class ControlActivity extends AppCompatActivity {
    private static final String TAG = "ControlActivity";

    // UI元素
    private RadioGroup speedRadioGroup;
    private RadioGroup durationRadioGroup;
    private Button forwardButton;
    private Button backwardButton;
    private Button leftTurnButton;
    private Button rightTurnButton;
    private Button leftShiftButton;
    private Button rightShiftButton;
    private Button stopButton;
    private Button cameraButton;
    private ImageView cameraImageView;
    private TextView cameraStatusText;
    private CheckBox followLineCheckbox;
    private Button viewEnvironmentDataButton;
    private TextView statusText;
    private EditText dockerIdEdit;
    private Button startObstacleButton;
    private Button stopObstacleButton;

    // 控制参数
    private int speed = 50;
    private float lastTime = -1; // -1表示持续
    private boolean continuousMode = false; // 持续运行模式

    // 运动状态常量（如果RosmasterLib中没有定义）
    private static final int MOTOR_LEFT_SHIFT = 5;  // 左平移
    private static final int MOTOR_RIGHT_SHIFT = 6; // 右平移

    // Rosmaster库
    private RosmasterLib rosmasterLib;
    private RosmasterCamera rosmasterCamera;
    private RosmasterHttpClient rosmasterHttpClient;
    private boolean isCameraRunning = false;
    private boolean isConnected = false;

    // TCP连接
    private Socket tcpSocket;
    private PrintWriter tcpWriter;
    private BufferedReader tcpReader;
    private boolean isTcpConnected = false;

    // 线程池和Handler
    private ExecutorService executorService;
    private Handler mainHandler;

    // 连接信息
    private String ipAddress;
    private int port;
    
    // 重试参数
    private static final int MAX_RETRIES = 3;
    private static final int RETRY_DELAY_MS = 500;

    // 添加一个变量来跟踪当前按下的按钮
    private Button currentPressedButton = null;

    // 添加安全定时器相关变量
    private Handler safetyHandler;
    private Runnable safetyRunnable;
    private static final long SAFETY_CHECK_INTERVAL = 3000; // 3秒检查一次
    private long lastCommandTime = 0;
    private static final long COMMAND_TIMEOUT = 5000; // 5秒无命令则停止

    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        
        // 强制使用横屏模式
        setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_LANDSCAPE);
        
        setContentView(R.layout.activity_control);

        // 从Intent获取连接信息
        ipAddress = getIntent().getStringExtra("IP_ADDRESS");
        port = getIntent().getIntExtra("PORT", 6000);

        // 初始化UI
        initUI();
        
        // 始终使用持续模式
        lastTime = -1;
        
        // 检查控制按钮是否正确初始化
        Log.d(TAG, "控制按钮初始化状态: " + 
              "前进=" + (forwardButton != null) + 
              ", 后退=" + (backwardButton != null) + 
              ", 左转=" + (leftTurnButton != null) + 
              ", 右转=" + (rightTurnButton != null) + 
              ", 左平移=" + (leftShiftButton != null) + 
              ", 右平移=" + (rightShiftButton != null) + 
              ", 停止=" + (stopButton != null));

        // 初始化线程池和Handler
        executorService = Executors.newCachedThreadPool();
        mainHandler = new Handler(Looper.getMainLooper());

        // 初始化安全检查机制
        initSafetyCheck();

        // 初始化Rosmaster库
        rosmasterLib = new RosmasterLib(RosmasterLib.CARTYPE_X1, true);
        Log.d(TAG, "使用小车类型CARTYPE_X1初始化RosmasterLib");
        
        // 初始化摄像头
        rosmasterCamera = new RosmasterCamera();

        // 设置状态文本
        statusText.setText("正在连接到 " + ipAddress + ":" + port);
        
        // 初始化连接
        initConnection();
    }
    
    private void initConnection() {
        executorService.execute(() -> {
            try {
                // 尝试TCP连接
                connectTcp();
                
                if (isTcpConnected) {
                    // TCP连接成功
                    isConnected = true;
                    mainHandler.post(() -> {
                        statusText.setText("已通过TCP连接到 " + ipAddress + ":" + port);
                        // 设置按钮事件
                        setupButtonListeners();
                        // 尝试启动摄像头
                        startCamera();
                    });
                    return;
                }
                
                // 如果TCP连接失败，尝试HTTP连接
                // 尝试多个端口
                int[] portsToTry = {port, 6000, 6001, 6500, 8080};
                boolean connected = false;
                
                for (int tryPort : portsToTry) {
                    try {
                        // 测试TCP连接
                        Socket socket = new Socket();
                        socket.connect(new java.net.InetSocketAddress(ipAddress, tryPort), 3000);
                        socket.close();
                        
                        // 创建Rosmaster HTTP客户端
                        rosmasterHttpClient = new RosmasterHttpClient(ipAddress, tryPort, true);
                        
                        // 测试连接
                        if (testConnection()) {
                            connected = true;
                            port = tryPort;
                            break;
                        }
                    } catch (Exception e) {
                        Log.e(TAG, "连接到端口 " + tryPort + " 失败: " + e.getMessage());
                    }
                }
                
                if (connected) {
                    isConnected = true;
                    mainHandler.post(() -> {
                        statusText.setText("已通过HTTP连接到 " + ipAddress + ":" + port);
                        // 设置按钮事件
                        setupButtonListeners();
                        // 尝试启动摄像头
                        startCamera();
                    });
                } else {
                    mainHandler.post(() -> {
                        statusText.setText("连接失败，请返回重试");
                        showToast("无法连接到小车，请检查IP地址和网络连接");
                    });
                }
            } catch (Exception e) {
                Log.e(TAG, "初始化连接失败: " + e.getMessage());
                mainHandler.post(() -> {
                    statusText.setText("连接错误: " + e.getMessage());
                    showToast("连接错误: " + e.getMessage());
                });
            }
        });
    }
    
    private void connectTcp() {
        int retryCount = 0;
        final int maxRetries = 3;
        
        while (retryCount < maxRetries) {
            try {
                Log.d(TAG, "尝试TCP连接到 " + ipAddress + ":" + port + " (尝试 " + (retryCount + 1) + "/" + maxRetries + ")");
                tcpSocket = new Socket();
                tcpSocket.connect(new java.net.InetSocketAddress(ipAddress, port), 5000);  // 增加连接超时到5秒
                tcpSocket.setSoTimeout(5000);  // 设置读取超时
                
                // 创建输入输出流
                tcpWriter = new PrintWriter(tcpSocket.getOutputStream(), true);
                tcpReader = new BufferedReader(new InputStreamReader(tcpSocket.getInputStream()));
                
                // 读取欢迎消息 - 添加超时处理
                String welcome = null;
                try {
                    welcome = tcpReader.readLine();
                    Log.d(TAG, "收到TCP服务器欢迎消息: " + welcome);
                } catch (java.net.SocketTimeoutException e) {
                    Log.w(TAG, "等待欢迎消息超时，继续执行");
                    // 即使没有欢迎消息也继续
                }
                
                // 发送测试命令 - 使用标准格式
                String testCommand = "$0015040000#";  // 停止命令
                Log.d(TAG, "发送TCP测试命令: " + testCommand);
                tcpWriter.println(testCommand);
                tcpWriter.flush();
                
                // 读取响应 - 添加超时处理
                String response = null;
                try {
                    response = tcpReader.readLine();
                    Log.d(TAG, "收到TCP服务器响应: " + response);
                } catch (java.net.SocketTimeoutException e) {
                    Log.w(TAG, "等待响应超时，但仍然认为连接成功");
                    // 即使没有响应也认为连接成功，因为有些服务器可能不返回响应
                }
                
                isTcpConnected = true;
                Log.d(TAG, "TCP连接成功");
                
                // 启动接收线程
                startTcpReceiveThread();
                
                return;
            } catch (java.net.ConnectException e) {
                Log.e(TAG, "TCP连接被拒绝: " + e.getMessage());
                retryCount++;
                if (retryCount < maxRetries) {
                    try {
                        Thread.sleep(1000); // 等待1秒后重试
                    } catch (InterruptedException ie) {
                        Thread.currentThread().interrupt();
                    }
                }
            } catch (java.net.SocketTimeoutException e) {
                Log.e(TAG, "TCP连接超时: " + e.getMessage());
                retryCount++;
                if (retryCount < maxRetries) {
                    try {
                        Thread.sleep(1000); // 等待1秒后重试
                    } catch (InterruptedException ie) {
                        Thread.currentThread().interrupt();
                    }
                }
            } catch (Exception e) {
                Log.e(TAG, "TCP连接失败: " + e.getMessage());
                retryCount++;
                if (retryCount < maxRetries) {
                    try {
                        Thread.sleep(1000); // 等待1秒后重试
                    } catch (InterruptedException ie) {
                        Thread.currentThread().interrupt();
                    }
                }
            }
        }
        
        // 所有重试都失败
        Log.e(TAG, "TCP连接失败，已尝试 " + maxRetries + " 次");
        closeTcpConnection();
    }
    
    private void startTcpReceiveThread() {
        executorService.execute(() -> {
            try {
                String line;
                while (isTcpConnected && tcpSocket != null && !tcpSocket.isClosed() && (line = tcpReader.readLine()) != null) {
                    Log.d(TAG, "收到TCP服务器消息: " + line);
                }
            } catch (Exception e) {
                if (isTcpConnected) {
                    Log.e(TAG, "TCP接收线程异常: " + e.getMessage());
                }
            } finally {
                closeTcpConnection();
            }
        });
    }
    
    private void closeTcpConnection() {
        isTcpConnected = false;
        
        try {
            if (tcpWriter != null) {
                tcpWriter.close();
                tcpWriter = null;
            }
            
            if (tcpReader != null) {
                tcpReader.close();
                tcpReader = null;
            }
            
            if (tcpSocket != null) {
                tcpSocket.close();
                tcpSocket = null;
            }
        } catch (Exception e) {
            Log.e(TAG, "关闭TCP连接异常: " + e.getMessage());
        }
    }
    
    private boolean testConnection() {
        try {
            // 发送测试命令
            String testCommand = rosmasterLib.setCarRunHttp(RosmasterLib.MOTOR_STOP, 0);
            String response = rosmasterHttpClient.sendHttpCommand(testCommand);
            Log.d(TAG, "测试连接响应: " + response);
            return true;
        } catch (Exception e) {
            Log.e(TAG, "测试连接失败: " + e.getMessage());
            return false;
        }
    }

    private void initUI() {
        speedRadioGroup = findViewById(R.id.speed_radio_group);
        durationRadioGroup = findViewById(R.id.duration_radio_group);
        forwardButton = findViewById(R.id.forward_button);
        backwardButton = findViewById(R.id.backward_button);
        leftTurnButton = findViewById(R.id.left_turn_button);
        rightTurnButton = findViewById(R.id.right_turn_button);
        leftShiftButton = findViewById(R.id.left_shift_button);
        rightShiftButton = findViewById(R.id.right_shift_button);
        stopButton = findViewById(R.id.stop_button);
        cameraButton = findViewById(R.id.camera_button);
        cameraImageView = findViewById(R.id.camera_image_view);
        cameraStatusText = findViewById(R.id.camera_status_text);
        followLineCheckbox = findViewById(R.id.follow_line_checkbox);
        viewEnvironmentDataButton = findViewById(R.id.view_environment_data_button);
        statusText = findViewById(R.id.status_text);
        dockerIdEdit = findViewById(R.id.docker_id_edit);
        startObstacleButton = findViewById(R.id.start_obstacle_button);
        stopObstacleButton = findViewById(R.id.stop_obstacle_button);

        // LED控制按钮
        // ledRedButton = findViewById(R.id.led_red_button);
        // ledGreenButton = findViewById(R.id.led_green_button);
        // ledBlueButton = findViewById(R.id.led_blue_button);
        // ledYellowButton = findViewById(R.id.led_yellow_button);
        // ledCyanButton = findViewById(R.id.led_cyan_button);
        // ledMagentaButton = findViewById(R.id.led_magenta_button);
        // ledWhiteButton = findViewById(R.id.led_white_button);
        // ledOffButton = findViewById(R.id.led_off_button);
        
        // 确保所有按钮都找到了
        if (forwardButton == null) Log.e(TAG, "前进按钮未找到");
        if (backwardButton == null) Log.e(TAG, "后退按钮未找到");
        if (leftTurnButton == null) Log.e(TAG, "左转按钮未找到");
        if (rightTurnButton == null) Log.e(TAG, "右转按钮未找到");
        if (leftShiftButton == null) Log.e(TAG, "左平移按钮未找到");
        if (rightShiftButton == null) Log.e(TAG, "右平移按钮未找到");
        if (stopButton == null) Log.e(TAG, "停止按钮未找到");
        if (cameraButton == null) Log.e(TAG, "摄像头按钮未找到");
        if (viewEnvironmentDataButton == null) Log.e(TAG, "查看环境数据按钮未找到");
        if (dockerIdEdit == null) Log.e(TAG, "Docker ID输入框未找到");
        if (startObstacleButton == null) Log.e(TAG, "启动避障按钮未找到");
        if (stopObstacleButton == null) Log.e(TAG, "停止避障按钮未找到");

        // 确保按钮可见
        if (forwardButton != null) forwardButton.setVisibility(View.VISIBLE);
        if (backwardButton != null) backwardButton.setVisibility(View.VISIBLE);
        if (leftTurnButton != null) leftTurnButton.setVisibility(View.VISIBLE);
        if (rightTurnButton != null) rightTurnButton.setVisibility(View.VISIBLE);
        if (leftShiftButton != null) leftShiftButton.setVisibility(View.VISIBLE);
        if (rightShiftButton != null) rightShiftButton.setVisibility(View.VISIBLE);
        if (stopButton != null) stopButton.setVisibility(View.VISIBLE);
        if (cameraButton != null) cameraButton.setVisibility(View.VISIBLE);
        if (dockerIdEdit != null) dockerIdEdit.setVisibility(View.VISIBLE);
        if (startObstacleButton != null) startObstacleButton.setVisibility(View.VISIBLE);
        if (stopObstacleButton != null) stopObstacleButton.setVisibility(View.VISIBLE);

        // 寻迹功能
        if (followLineCheckbox != null) {
            followLineCheckbox.setOnCheckedChangeListener((buttonView, isChecked) -> {
                if (rosmasterHttpClient == null) {
                    Toast.makeText(this, "请先连接到Rosmaster", Toast.LENGTH_SHORT).show();
                    buttonView.setChecked(false);
                    return;
                }

                executorService.execute(() -> {
                    try {
                        // 使用RosmasterLib的标准寻迹命令
                        String command = rosmasterLib.setFollowLineHttp(isChecked ? 1 : 0);
                        Log.d(TAG, "发送寻迹命令: " + command + ", isChecked: " + isChecked);

                        // 先发送速度命令
                        String speedCommand = String.format("$%02x%02x%02x%02x%02x#",
                            0, 0x15, 0x0A, speed,
                            (0 + 0x15 + 0x0A + speed) % 256);
                        sendCommand(speedCommand);
                        Log.d(TAG, "发送速度命令: " + speedCommand + ", speed: " + speed);

                        // 等待速度命令处理
                        Thread.sleep(100);

                        // 发送寻迹命令
                        sendCommand(command);

                        runOnUiThread(() -> {
                            Toast.makeText(this, isChecked ? "已启动寻迹" : "已停止寻迹", Toast.LENGTH_SHORT).show();
                        });
                    } catch (Exception e) {
                        Log.e(TAG, "发送寻迹命令时出错: " + e.toString());
                        runOnUiThread(() -> {
                            Toast.makeText(this, "发送命令失败: " + e.getMessage(), Toast.LENGTH_SHORT).show();
                            buttonView.setChecked(!isChecked); // 恢复原来的状态
                        });
                    }
                });
            });
        }
        if (viewEnvironmentDataButton != null) viewEnvironmentDataButton.setVisibility(View.VISIBLE);
        viewEnvironmentDataButton.setOnClickListener(v -> showEnvironmentDataDialog());

    }
    private void showEnvironmentDataDialog() {
        // 从SharedPreferences中读取并显示环境数据
        SharedPreferences sharedPreferences = getSharedPreferences("DeviceShadowData", MODE_PRIVATE);

        // 读取存储的各个值
        int gasValue = sharedPreferences.getInt("Gas_value", -1);
        String unit = sharedPreferences.getString("Unit", "N/A");
        int illumination = sharedPreferences.getInt("Illumination", -1);
        int smoke = sharedPreferences.getInt("Smoke", -1);
        int pm25 = sharedPreferences.getInt("PM25", -1);
        String eventTime = sharedPreferences.getString("Event_time", "N/A");

        // 将 UTC 时间转为北京时间
        String localEventTime = convertUTCToBeijing(eventTime);

        // 构建显示的数据字符串
        String data =
                "气体值: " + gasValue + "\n" +
                        "光照: " + illumination + "\n" +
                        "烟雾: " + smoke + "\n" +
                        "PM2.5: " + pm25 + "\n" +
                        "最后更新时间: " + localEventTime;

        // 创建一个 Dialog 来显示环境数据，使用自定义布局
        AlertDialog.Builder builder = new AlertDialog.Builder(this);

        // 设置自定义布局
        View customView = getLayoutInflater().inflate(R.layout.dialog_environment_data, null);
        TextView dataText = customView.findViewById(R.id.data_text);  // 确保引用了正确的 ID
        Button closeButton = customView.findViewById(R.id.close_button);  // 确保引用了正确的 ID

        // 设置数据
        dataText.setText(data);



        // 将自定义视图设置为 Dialog 的内容
        builder.setView(customView);

        // 创建并显示 Dialog
        AlertDialog dialog = builder.create();  // 声明并创建 AlertDialog 对象
        // 设置按钮点击事件
        closeButton.setOnClickListener(v -> {
            // 关闭按钮的点击事件
            dialog.dismiss(); // 这里我们现在能够正常引用 dialog 了
        });
        dialog.show();
    }

    // 将 UTC 时间转换为北京时间
    private String convertUTCToBeijing(String utcTime) {
        try {
            // 解析 UTC 时间
            SimpleDateFormat utcFormat = new SimpleDateFormat("yyyyMMdd'T'HHmmss'Z'", Locale.US);
            utcFormat.setTimeZone(TimeZone.getTimeZone("UTC"));
            Date date = utcFormat.parse(utcTime);

            // 转换为北京时间
            SimpleDateFormat beijingFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.CHINA);
            beijingFormat.setTimeZone(TimeZone.getTimeZone("GMT+08:00"));
            return beijingFormat.format(date);
        } catch (ParseException e) {
            Log.e(TAG, "时间转换失败: " + e.getMessage());
            return utcTime;  // 如果解析失败，返回原始的 UTC 时间
        }
    }
    private void setupButtonListeners() {
        // 设置速度选择
        speedRadioGroup.setOnCheckedChangeListener((group, checkedId) -> {
            if (checkedId == R.id.speed_10) {
                speed = 10;
            } else if (checkedId == R.id.speed_20) {
                speed = 20;
            } else if (checkedId == R.id.speed_30) {
                speed = 30;
            } else if (checkedId == R.id.speed_50) {
                speed = 50;
            }
            Log.d(TAG, "设置速度为: " + speed);
            
            // 立即发送速度命令
            if (isConnected) {
                executorService.execute(() -> {
                    try {
                        String speedCommand = String.format("$%02x%02x%02x%02x%02x#", 
                                0, 0x15, 0x0A, speed,
                                (0 + 0x15 + 0x0A + speed) % 256);
                        Log.d(TAG, "速度改变，发送速度命令: " + speedCommand);
                        sendCommand(speedCommand);
                        
                        // 如果当前有按钮被按下，重新应用当前状态
                        if (currentPressedButton != null) {
                            int currentState = (int) currentPressedButton.getTag();
                            Thread.sleep(50);  // 短暂等待确保速度命令被处理
                            String command = generateCommandForState(currentState);
                            sendCommand(command);
                            Log.d(TAG, "速度改变，重新应用当前状态: " + currentState);
                        }
                    } catch (Exception e) {
                        Log.e(TAG, "发送速度命令失败: " + e.getMessage());
                    }
                });
            }
        });
        
        // 设置寻迹复选框
        // followLineCheckbox.setOnCheckedChangeListener((buttonView, isChecked) -> {
        //     if (isConnected) {
        //         try {
        //             String command = isChecked ? "$0360040001#" : "$0360040000#";
        //             sendCommand(command);
        //             Log.d(TAG, "发送寻迹命令: " + command);
        //         } catch (Exception e) {
        //             Log.e(TAG, "发送寻迹命令失败: " + e.getMessage());
        //             showToast("发送寻迹命令失败: " + e.getMessage());
        //         }
        //     } else {
        //         showToast("未连接到小车，请先连接");
        //         buttonView.setChecked(!isChecked); // 恢复原来的状态
        //     }
        // });
        
        // 设置摄像头按钮
        if (cameraButton != null) {
            cameraButton.setOnClickListener(v -> {
                startCamera();
            });
        }

        // 设置自动避障按钮点击事件
        if (startObstacleButton != null) {
            startObstacleButton.setOnClickListener(v -> {
                String dockerId = dockerIdEdit.getText().toString().trim();
                if (dockerId.isEmpty()) {
                    Toast.makeText(this, "请输入Docker ID", Toast.LENGTH_SHORT).show();
                    return;
                }

                // 发送启动避障命令
                executorService.execute(() -> {
                    try {
                        String command = "$START_OBSTACLE " + dockerId + "#";
                        Log.d(TAG, "发送启动避障命令: " + command);
                        rosmasterHttpClient.sendCommand(command);

                        runOnUiThread(() -> {
                            Toast.makeText(ControlActivity.this, "已启动避障服务", Toast.LENGTH_SHORT).show();
                        });
                    } catch (Exception e) {
                        Log.e(TAG, "发送启动避障命令时出错: " + e.toString());
                        runOnUiThread(() -> {
                            Toast.makeText(ControlActivity.this, "发送命令失败: " + e.getMessage(), Toast.LENGTH_SHORT).show();
                        });
                    }
                });
            });
        }

        // 设置停止避障按钮点击事件
        if (stopObstacleButton != null) {
            stopObstacleButton.setOnClickListener(v -> {
                String dockerId = dockerIdEdit.getText().toString().trim();
                if (dockerId.isEmpty()) {
                    Toast.makeText(this, "请输入Docker ID", Toast.LENGTH_SHORT).show();
                    return;
                }

                // 发送停止避障命令
                executorService.execute(() -> {
                    try {
                        String command = "$STOP_OBSTACLE " + dockerId + "#";
                        Log.d(TAG, "发送停止避障命令: " + command);
                        rosmasterHttpClient.sendCommand(command);

                        runOnUiThread(() -> {
                            Toast.makeText(ControlActivity.this, "已停止避障服务", Toast.LENGTH_SHORT).show();
                        });
                    } catch (Exception e) {
                        Log.e(TAG, "发送停止避障命令时出错: " + e.toString());
                        runOnUiThread(() -> {
                            Toast.makeText(ControlActivity.this, "发送命令失败: " + e.getMessage(), Toast.LENGTH_SHORT).show();
                        });
                    }
                });
            });
        }

        // 设置控制按钮
        updateControlButtonListeners();
    }
    
    private void updateControlButtonListeners() {
        // 设置所有控制按钮
        setupControlButton(forwardButton, RosmasterLib.MOTOR_FORWARD);
        setupControlButton(backwardButton, RosmasterLib.MOTOR_BACKWARD);
        setupControlButton(leftTurnButton, RosmasterLib.MOTOR_LEFT);
        setupControlButton(rightTurnButton, RosmasterLib.MOTOR_RIGHT);
        setupControlButton(leftShiftButton, MOTOR_LEFT_SHIFT);
        setupControlButton(rightShiftButton, MOTOR_RIGHT_SHIFT);
        setupControlButton(stopButton, RosmasterLib.MOTOR_STOP);
    }
    
    private void setupControlButton(Button button, int motorState) {
        if (button == null) {
            Log.e(TAG, "按钮为null，无法设置事件");
            return;
        }

        // 存储状态值到按钮的Tag中
        button.setTag(motorState);
        
        // 长按控制模式：按下时移动，松开时停止
        button.setOnTouchListener((v, event) -> {
            if (!isConnected) {
                showToast("请先连接到小车");
                return false;
            }

            try {
                if (event.getAction() == MotionEvent.ACTION_DOWN) {
                    Log.d(TAG, "按钮按下: " + button.getId() + ", 状态: " + motorState);
                    // 如果有其他按钮被按下，先恢复它的背景
                    if (currentPressedButton != null && currentPressedButton != button) {
                        currentPressedButton.getBackground().clearColorFilter();
                        // 确保之前按下的按钮也会触发停止命令
                        executeCommand(RosmasterLib.MOTOR_STOP);
                    }
                    currentPressedButton = button;
                    executeCommand(motorState);
                    // 设置按钮背景为深色，表示已选中
                    button.getBackground().setColorFilter(0x80000000, android.graphics.PorterDuff.Mode.SRC_ATOP);
                    return true;
                } else if (event.getAction() == MotionEvent.ACTION_UP || event.getAction() == MotionEvent.ACTION_CANCEL) {
                    Log.d(TAG, "按钮释放: " + button.getId());
                    // 恢复按钮背景
                    button.getBackground().clearColorFilter();
                    // 只有当这个按钮是当前按下的按钮时，才发送停止命令
                    if (currentPressedButton == button) {
                        currentPressedButton = null;
                        if (motorState != RosmasterLib.MOTOR_STOP) {
                            executeCommand(RosmasterLib.MOTOR_STOP);
                        }
                    }
                    return true;
                }
            } catch (Exception e) {
                Log.e(TAG, "按钮事件处理异常: " + e.getMessage());
                // 出现任何异常都确保小车停止
                if (currentPressedButton != null) {
                    currentPressedButton.getBackground().clearColorFilter();
                    currentPressedButton = null;
                }
                executeCommand(RosmasterLib.MOTOR_STOP);
            }
            return false;
        });
    }

    /**
     * 根据状态生成命令字符串
     * @param state 运动状态
     * @return 命令字符串
     */
    private String generateCommandForState(int state) {
        // 使用标准命令格式 - 使用类型0（通用类型）
        return String.format("$%02x%02x%02x%02x%02x#",
                0, 0x15, 0x04, state,
                (0 + 0x15 + 0x04 + state) % 256);
    }

    private void executeCommand(int state) {
        if (!isConnected) {
            showToast("未连接到小车，请先连接");
            return;
        }
        
        executorService.execute(() -> {
            try {
                // 更新最后命令时间
                lastCommandTime = System.currentTimeMillis();
                
                // 先发送速度命令 - 使用类型0（通用类型）
                String speedCommand = String.format("$%02x%02x%02x%02x%02x#", 
                        0, 0x15, 0x0A, speed,
                        (0 + 0x15 + 0x0A + speed) % 256);
                
                Log.d(TAG, "发送速度命令: " + speedCommand + ", 速度值: " + speed);
                sendCommand(speedCommand);
                
                // 等待一小段时间确保速度命令被处理
                Thread.sleep(50);
                
                // 生成动作命令
                String command = generateCommandForState(state);
                
                Log.d(TAG, "发送动作命令: " + command + " 用于状态: " + state);
                sendCommand(command);
                
                // 记录命令发送成功
                Log.d(TAG, "命令已发送: 速度=" + speed + ", 动作=" + state);
            } catch (Exception e) {
                Log.e(TAG, "发送命令失败", e);
                showToast("发送命令失败: " + e.getMessage());
            }
        });
    }
    
    private void sendCommand(String command) throws IOException {
        // 首先尝试通过TCP发送
        if (isTcpConnected && tcpWriter != null) {
            try {
                Log.d(TAG, "通过TCP发送命令: " + command);
                // 确保命令格式正确（以$开头，以#结尾）
                if (!command.startsWith("$")) {
                    command = "$" + command;
                }
                if (!command.endsWith("#")) {
                    command = command + "#";
                }
                // 直接发送命令，不添加HTTP头
                tcpWriter.println(command);
                tcpWriter.flush(); // 确保立即发送
                return;
            } catch (Exception e) {
                Log.e(TAG, "TCP发送失败，尝试HTTP: " + e.getMessage());
                closeTcpConnection();
            }
        }
        
        // 如果TCP失败或未连接，尝试HTTP
        if (rosmasterHttpClient != null) {
            sendCommandWithRetry(command);
        } else {
            // 创建新的HTTP客户端尝试发送
            try {
                Log.d(TAG, "创建新的HTTP客户端尝试发送命令");
                rosmasterHttpClient = new RosmasterHttpClient(ipAddress, port, true);
                sendCommandWithRetry(command);
            } catch (Exception e) {
                Log.e(TAG, "创建新HTTP客户端失败: " + e.getMessage());
                throw new IOException("没有可用的连接方式");
            }
        }
    }
    
    private void sendCommandWithRetry(String command) throws IOException {
        Exception lastException = null;
        
        for (int i = 0; i < MAX_RETRIES; i++) {
            try {
                String response = rosmasterHttpClient.sendHttpCommand(command);
                Log.d(TAG, "发送命令成功: " + command + ", 响应: " + response);
                
                // 检查是否是TCP模式响应
                if ("TCP_MODE_COMMAND_SENT".equals(response)) {
                    Log.d(TAG, "命令已通过TCP模式发送");
                }
                return;
            } catch (IOException e) {
                lastException = e;
                Log.e(TAG, "发送命令失败 (尝试 " + (i+1) + "/" + MAX_RETRIES + "): " + e.getMessage());
                
                // 检查是否是TCP服务器欢迎消息导致的错误
                if (e.getMessage() != null && e.getMessage().contains("Welcome to Rosmaster TCP Server")) {
                    Log.d(TAG, "检测到TCP服务器，尝试建立TCP连接");
                    try {
                        // 尝试建立TCP连接
                        connectTcp();
                        if (isTcpConnected && tcpWriter != null) {
                            // 通过TCP发送命令
                            tcpWriter.println(command);
                            tcpWriter.flush();
                            Log.d(TAG, "通过新建立的TCP连接发送命令成功");
                            return;
                        }
                    } catch (Exception tcpEx) {
                        Log.e(TAG, "尝试建立TCP连接失败: " + tcpEx.getMessage());
                    }
                }
                
                try {
                    Thread.sleep(RETRY_DELAY_MS);
                } catch (InterruptedException ie) {
                    Thread.currentThread().interrupt();
                    throw new IOException("命令发送被中断", ie);
                }
            }
        }
        
        // 所有重试都失败
        if (lastException != null) {
            throw new IOException("发送命令失败，已重试" + MAX_RETRIES + "次: " + lastException.getMessage(), lastException);
        } else {
            throw new IOException("发送命令失败，未知原因");
        }
    }

    private void executeCommandWithDuration(int state) {
        if (!isConnected) {
            showToast("未连接到小车，请先连接");
            return;
        }
        
        executorService.execute(() -> {
            try {
                // 发送移动命令
                String moveCommand = rosmasterLib.setCarRunHttp(state, speed);
                sendCommand(moveCommand);
                
                // 等待指定时间
                Thread.sleep((long) (lastTime * 1000));
                
                // 发送停止命令
                String stopCommand = rosmasterLib.setCarRunHttp(RosmasterLib.MOTOR_STOP, speed);
                sendCommand(stopCommand);
            } catch (Exception e) {
                Log.e(TAG, "定时命令执行失败", e);
                showToast("定时命令执行失败: " + e.getMessage());
            }
        });
    }

    private void startCamera() {
        executorService.execute(() -> {
            try {
                if (!isCameraRunning) {
                    // 更新UI状态
                    mainHandler.post(() -> {
                        cameraStatusText.setText("正在连接摄像头...");
                        cameraStatusText.setVisibility(View.VISIBLE);
                        cameraImageView.setVisibility(View.VISIBLE);
                        if (cameraButton != null) {
                            cameraButton.setEnabled(false);
                        }
                    });
                    
                    // 先尝试建立直接的RTSP流连接，这是最实时的方式
                    boolean connected = tryConnectRTSPStream();
                    
                    if (!connected) {
                        // 如果RTSP连接失败，尝试启动摄像头服务器并使用HTTP流
                        connected = tryStartCameraServer();
                    }
                    
                    if (!connected) {
                        mainHandler.post(() -> {
                            cameraStatusText.setText("未连接摄像头");
                            cameraStatusText.setVisibility(View.VISIBLE);
                            showToast("无法连接到摄像头，请确保摄像头服务已启动");
                            if (cameraButton != null) {
                                cameraButton.setText("启动摄像头");
                                cameraButton.setEnabled(true);
                            }
                        });
                    }
                } else {
                    // 停止摄像头
                    stopCamera();
                }
            } catch (Exception e) {
                Log.e(TAG, "摄像头操作失败: " + e.getMessage());
                mainHandler.post(() -> {
                    cameraStatusText.setText("摄像头错误: " + e.getMessage());
                    cameraStatusText.setVisibility(View.VISIBLE);
                    if (cameraButton != null) {
                        cameraButton.setEnabled(true);
                    }
                });
            }
        });
    }
    
    /**
     * 尝试建立直接的RTSP流连接，这是最实时的方式
     * @return 是否连接成功
     */
    private boolean tryConnectRTSPStream() throws InterruptedException {
        Log.d(TAG, "尝试建立RTSP流连接");
        
        // 尝试使用RTSP协议直接连接摄像头
        String[] rtspUrls = {
            "rtsp://" + ipAddress + ":8554/live", // 标准RTSP端口
            "rtsp://" + ipAddress + ":554/live",  // 另一个标准RTSP端口
            "rtsp://" + ipAddress + ":7070/video" // 自定义RTSP端口
        };
        
        for (String rtspUrl : rtspUrls) {
            try {
                Log.d(TAG, "尝试连接RTSP: " + rtspUrl);
                
                // 发送命令启动RTSP服务器
                if (isTcpConnected && tcpWriter != null) {
                    String startRtspCommand = "$0360060001#"; // 启动RTSP服务器命令
                    tcpWriter.println(startRtspCommand);
                    tcpWriter.flush();
                    Log.d(TAG, "发送启动RTSP服务器命令: " + startRtspCommand);
                    Thread.sleep(2000); // 给服务器一些时间启动
                }
                
                // 连接RTSP流
                rosmasterCamera.startCamera(rtspUrl, new RosmasterCamera.CameraCallback() {
                    @Override
                    public void onFrameReceived(Bitmap bitmap) {
                        // 显示摄像头画面
                        mainHandler.post(() -> {
                            if (bitmap != null) {
                                cameraImageView.setImageBitmap(bitmap);
                                cameraStatusText.setVisibility(View.GONE);
                                isCameraRunning = true;
                                if (cameraButton != null) {
                                    cameraButton.setText("停止摄像头");
                                    cameraButton.setEnabled(true);
                                }
                            }
                        });
                    }
                    
                    @Override
                    public void onError(String error) {
                        // 显示错误
                        mainHandler.post(() -> {
                            cameraStatusText.setText("RTSP错误: " + error);
                            cameraStatusText.setVisibility(View.VISIBLE);
                            if (cameraButton != null) {
                                cameraButton.setEnabled(true);
                            }
                        });
                    }
                });
                
                // 等待一段时间看是否成功连接
                for (int i = 0; i < 15; i++) {  // 增加等待时间到4.5秒
                    Thread.sleep(300); // 每300ms检查一次
                    if (isCameraRunning) {
                        break;
                    }
                }
                
                if (isCameraRunning) {
                    Log.d(TAG, "成功连接到RTSP流: " + rtspUrl);
                    return true;
                } else {
                    // 停止当前尝试
                    rosmasterCamera.stopCamera();
                    Log.d(TAG, "连接RTSP流失败，尝试下一个URL");
                }
            } catch (Exception e) {
                Log.e(TAG, "连接RTSP流失败: " + e.getMessage());
            }
        }
        
        return false;
    }
    
    private boolean tryStartCameraServer() throws InterruptedException {
        Log.d(TAG, "尝试启动摄像头服务器并使用HTTP流");
        
        // 先尝试发送命令启动摄像头服务器
        if (isTcpConnected && tcpWriter != null) {
            try {
                // 发送摄像头初始化命令
                String startCameraCommand = "$0360040001#"; // 启动摄像头命令
                tcpWriter.println(startCameraCommand);
                tcpWriter.flush();
                Log.d(TAG, "发送启动摄像头命令: " + startCameraCommand);
                // 给服务器一些时间启动摄像头
                Thread.sleep(2000);
            } catch (Exception e) {
                Log.e(TAG, "发送启动摄像头命令失败: " + e.getMessage());
                // 继续尝试连接摄像头，即使命令发送失败
            }
        }
        
        // 参考camera_rosmaster.py中的实现，Rosmaster小车支持多种摄像头类型
        // 尝试多个可能的摄像头URL格式
        String[] cameraUrls = {
            // 默认HTTP流URL (普通USB摄像头)
            "http://" + ipAddress + ":6500/?action=stream",
            // 深度摄像头 (TYPE_DEPTH_CAMERA)
            "http://" + ipAddress + ":6500/depth_stream",
            // USB摄像头 (TYPE_USB_CAMERA)
            "http://" + ipAddress + ":6500/usb_stream",
            // 广角摄像头 (TYPE_WIDE_ANGLE_CAMERA)
            "http://" + ipAddress + ":6500/wide_angle_stream",
            // 备用端口
            "http://" + ipAddress + ":8080/?action=stream",
            // 直接使用IP地址（摄像头库会添加默认端口和路径）
            "http://" + ipAddress + ":6500/stream.mjpg",
            // 其他可能的格式
            "http://" + ipAddress + ":8080/stream.mjpg",
            "http://" + ipAddress + ":8081/?action=stream"
        };
        
        boolean connected = false;
        for (String cameraUrl : cameraUrls) {
            try {
                Log.d(TAG, "尝试连接摄像头: " + cameraUrl);
                
                // 连接摄像头流
                rosmasterCamera.startCamera(cameraUrl, new RosmasterCamera.CameraCallback() {
                    @Override
                    public void onFrameReceived(Bitmap bitmap) {
                        // 显示摄像头画面
                        mainHandler.post(() -> {
                            if (bitmap != null) {
                                cameraImageView.setImageBitmap(bitmap);
                                cameraStatusText.setVisibility(View.GONE);
                                isCameraRunning = true;
                                if (cameraButton != null) {
                                    cameraButton.setText("停止摄像头");
                                    cameraButton.setEnabled(true);
                                }
                            }
                        });
                    }
                    
                    @Override
                    public void onError(String error) {
                        // 显示错误
                        mainHandler.post(() -> {
                            cameraStatusText.setText("摄像头错误: " + error);
                            cameraStatusText.setVisibility(View.VISIBLE);
                            if (cameraButton != null) {
                                cameraButton.setEnabled(true);
                            }
                        });
                    }
                });
                
                // 等待一段时间看是否成功连接
                for (int i = 0; i < 15; i++) {  // 增加等待时间到4.5秒
                    Thread.sleep(300); // 每300ms检查一次
                    if (isCameraRunning) {
                        break;
                    }
                }
                
                if (isCameraRunning) {
                    Log.d(TAG, "成功连接到摄像头流: " + cameraUrl);
                    return true;
                } else {
                    // 停止当前尝试
                    rosmasterCamera.stopCamera();
                    Log.d(TAG, "连接摄像头流失败，尝试下一个URL");
                }
            } catch (Exception e) {
                Log.e(TAG, "连接摄像头流失败: " + e.getMessage());
            }
        }
        
        return false;
    }
    
    /**
     * 测试URL是否可达
     * @param urlString URL字符串
     * @return 是否可达
     */
    private boolean pingUrl(String urlString) {
        try {
            // 确保URL格式正确
            if (!urlString.startsWith("http://")) {
                urlString = "http://" + urlString;
            }
            
            // 如果URL不包含路径，添加/
            if (!urlString.substring(urlString.indexOf("://") + 3).contains("/")) {
                urlString = urlString + "/";
            }
            
            URL url = new URL(urlString);
            HttpURLConnection connection = (HttpURLConnection) url.openConnection();
            connection.setConnectTimeout(2000); // 2秒超时
            connection.setReadTimeout(2000);
            connection.setRequestMethod("HEAD"); // 只获取头信息
            
            int responseCode = connection.getResponseCode();
            Log.d(TAG, "Ping URL " + urlString + " 响应码: " + responseCode);
            
            return responseCode < 400; // 任何非错误响应都认为可达
        } catch (Exception e) {
            Log.d(TAG, "Ping URL " + urlString + " 失败: " + e.getMessage());
            return false;
        }
    }

    private void stopCamera() {
        if (isCameraRunning) {
            Log.d(TAG, "停止摄像头");
            rosmasterCamera.stopCamera();
            isCameraRunning = false;
            
            mainHandler.post(() -> {
                cameraStatusText.setText("摄像头已停止");
                cameraStatusText.setVisibility(View.VISIBLE);
                cameraImageView.setImageResource(android.R.color.transparent);
                if (cameraButton != null) {
                    cameraButton.setText("启动摄像头");
                }
            });
            
            // 如果使用TCP连接，发送停止摄像头命令
            if (isTcpConnected && tcpWriter != null) {
                try {
                    String stopCameraCommand = "$0360040000#";
                    tcpWriter.println(stopCameraCommand);
                    tcpWriter.flush();
                    Log.d(TAG, "发送停止摄像头命令: " + stopCameraCommand);
                } catch (Exception e) {
                    Log.e(TAG, "发送停止摄像头命令失败: " + e.getMessage());
                }
            }
        }
    }

    private void showToast(String message) {
        mainHandler.post(() -> {
            Toast.makeText(this, message, Toast.LENGTH_SHORT).show();
        });
    }

    // 初始化安全检查机制
    private void initSafetyCheck() {
        safetyHandler = new Handler(Looper.getMainLooper());
        safetyRunnable = new Runnable() {
            @Override
            public void run() {
                // 检查是否长时间没有命令，但小车可能仍在移动
                if (currentPressedButton == null && 
                    System.currentTimeMillis() - lastCommandTime > COMMAND_TIMEOUT) {
                    // 发送停止命令
                    executorService.execute(() -> {
                        try {
                            String stopCommand = generateCommandForState(RosmasterLib.MOTOR_STOP);
                            sendCommand(stopCommand);
                            Log.d(TAG, "安全检查：长时间无命令，发送停止命令");
                        } catch (Exception e) {
                            Log.e(TAG, "安全检查发送停止命令失败: " + e.getMessage());
                        }
                    });
                }
                
                // 再次安排检查
                safetyHandler.postDelayed(this, SAFETY_CHECK_INTERVAL);
            }
        };
        
        // 开始安全检查
        safetyHandler.postDelayed(safetyRunnable, SAFETY_CHECK_INTERVAL);
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        
        // 停止安全检查
        if (safetyHandler != null && safetyRunnable != null) {
            safetyHandler.removeCallbacks(safetyRunnable);
        }
        
        // 停止小车
        if (isConnected) {
            try {
                // 关闭寻迹功能
                String followLineOffCommand = rosmasterLib.setFollowLineHttp(0);
                sendCommand(followLineOffCommand);
                Log.d(TAG, "已关闭寻迹功能");
                
                // 停止小车
                String stopCommand = rosmasterLib.setCarRunHttp(RosmasterLib.MOTOR_STOP, 0);
                sendCommand(stopCommand);
                Log.d(TAG, "已停止小车");
            } catch (Exception e) {
                Log.e(TAG, "程序退出时停止小车失败: " + e.getMessage());
            }
        }
        
        // 停止摄像头
        stopCamera();
        
        // 关闭TCP连接
        closeTcpConnection();
        
        // 关闭线程池
        if (executorService != null) {
            executorService.shutdown();
        }
    }

    @Override
    protected void onPause() {
        super.onPause();
        
        // 应用进入后台时，确保小车停止
        if (isConnected) {
            try {
                // 清除当前按下的按钮状态
                if (currentPressedButton != null) {
                    mainHandler.post(() -> {
                        currentPressedButton.getBackground().clearColorFilter();
                    });
                    currentPressedButton = null;
                }
                
                // 发送停止命令
                String stopCommand = generateCommandForState(RosmasterLib.MOTOR_STOP);
                sendCommand(stopCommand);
                Log.d(TAG, "应用进入后台，发送停止命令");
            } catch (Exception e) {
                Log.e(TAG, "应用暂停时停止小车失败: " + e.getMessage());
            }
        }
        
        // 暂停安全检查
        if (safetyHandler != null && safetyRunnable != null) {
            safetyHandler.removeCallbacks(safetyRunnable);
        }
    }
    
    @Override
    protected void onResume() {
        super.onResume();
        
        // 恢复安全检查
        if (safetyHandler != null && safetyRunnable != null) {
            safetyHandler.postDelayed(safetyRunnable, SAFETY_CHECK_INTERVAL);
        }
        
        // 应用恢复前台时，确保小车处于停止状态
        if (isConnected) {
            try {
                String stopCommand = generateCommandForState(RosmasterLib.MOTOR_STOP);
                sendCommand(stopCommand);
                Log.d(TAG, "应用恢复前台，确保小车停止");
            } catch (Exception e) {
                Log.e(TAG, "应用恢复时确保小车停止失败: " + e.getMessage());
            }
        }
    }

    // 添加触摸事件安全保障机制
    @Override
    public boolean dispatchTouchEvent(MotionEvent event) {
        // 当触摸事件结束（手指抬起或取消）时，确保小车停止
        if (event.getAction() == MotionEvent.ACTION_UP || 
            event.getAction() == MotionEvent.ACTION_CANCEL || 
            event.getAction() == MotionEvent.ACTION_OUTSIDE) {
            
            // 如果有按钮被按下，恢复其状态并停止小车
            if (currentPressedButton != null) {
                currentPressedButton.getBackground().clearColorFilter();
                currentPressedButton = null;
                
                // 在后台线程中执行停止命令
                executorService.execute(() -> {
                    try {
                        String stopCommand = generateCommandForState(RosmasterLib.MOTOR_STOP);
                        sendCommand(stopCommand);
                        Log.d(TAG, "触摸事件结束，发送停止命令");
                    } catch (Exception e) {
                        Log.e(TAG, "发送停止命令失败: " + e.getMessage());
                    }
                });
            }
        }
        
        return super.dispatchTouchEvent(event);
    }
} 